#include "boids.h"
#include "AE.h"
#include <iostream>


Boid::Boid(const c8 *meshFilePath, const c8 *textureFilePath, irr::core::aabbox3d<f32>& containmentBox)
{
	node = makeAnimatedSimpleObject(meshFilePath, textureFilePath);
	location = BoidPosition3d(3000.0f,-3000.0f, 3000.0f, true);
	velocity = BoidVec3d(0.0f,0.0f,0.0f);
	boundaryBox =  containmentBox;
	velocity+= BoidVec3d((int)rand()%2 - .5f,(int)rand()%2 - .5f, (int)rand()%2 - .5f);
	#ifdef DEBUG
		std::cout<<location.x<<", "<< location.y<< ", "<< location.z<<std::endl;
	#endif
}

void Boid::updateBoid(std::vector<Boid*> boidList, float cohesionRange,float cohesionScalingFactor, float seperationRange,
float seperationScalingFactor, float alignmentRange, float alignmentScalingFactor,  int boidListLength)
{
	
	velocity += cohesion(boidList, cohesionRange, cohesionScalingFactor, boidListLength);
	velocity += seperation(boidList, seperationRange, seperationScalingFactor, boidListLength);
	velocity += alignment(boidList, alignmentRange, alignmentScalingFactor, boidListLength);


	remainWithinBoundary(1.0f, 1000.0f);
	limitSpeed(30.0f);
	

	
	location.movePosition(velocity);
	updateNode();//LAST THING TO DO IN UPDATE
	
}

void Boid::limitSpeed(float sensitivity)
{
	if (velocity.length() > sensitivity)
	{
	  
	  velocity.normalize();
	  velocity*=sensitivity;
	}

}
void Boid::updateNode()
{

	//Setting rotation
	vector3df nextPosition = vector3df(location.x, location.y, location.z);
	vector3df rotation;
	vector3df position = nextPosition - node->getAbsolutePosition(); //end - start
	rotation = position.getHorizontalAngle();
	rotation.Z = 0;
	node->setRotation(rotation);

	//setting position
	node->setPosition(nextPosition);

}

void Boid::remainWithinBoundary(float responsiveness, float scalingFactor)
{

	//Bounding Box Method
	if (!boundaryBox.isPointInside(node->getPosition()))
	{
		BoidVec3d vec = BoidVec3d(location, convertIrrVecToBoidPos(boundaryBox.getCenter()));
		vec.normalize();
		velocity += (vec*=responsiveness);
	}


	/*
	
	//! stores all 8 edges of the box into a array
	//! \param edges: Pointer to array of 8 edges
	
	Edges are stored in this way:
	Hey, am I an ascii artist, or what? :) niko.
          /3--------/7
         /  |      / |
        /   |     /  |
        1---------5  |
        |   2- - -| -6
        |  /      |  /
        |/        | /
        0---------4/ 
	

	vector3df* edgesList[7];


	boundaryBox.getEdges(edgesList);

	for (int i = 0; i <= 6; i++)
	{
		std::cout<<edgesList[i].X<<edgesList[i].Y<<edgesList[i].Z<<std::endl;
	
	}
	*/

}

BoidVec3d Boid::seperation(std::vector<Boid*> boidList, float range, float scalingFactor, int boidListLength)
{
	BoidVec3d totalOfVectors = BoidVec3d(0.0f, 0.0f, 0.0f);
	for (int i = 0; i < boidListLength; i++)
	{
	  
		float distance = location.getDistance(boidList[i]->location);//location.getDistance(eachBoid.location);
	  
	  if (distance != 0)
	  {
		if (distance < range)
		{
			
			BoidVec3d vec = BoidVec3d(boidList[i]->location, location);
			vec.normalize();

			vec/=distance;
			totalOfVectors+=vec;

		  
		  
		}
	  }
	}
	
	totalOfVectors*=scalingFactor;
	
	return totalOfVectors;
	
	
}
BoidVec3d Boid::alignment(std::vector<Boid*> boidList, float range, float scalingFactor, int boidListLength)
{

	int numOfBoidsNearby = 0;
	BoidVec3d sumOfVecs(0.0f, 0.0f, 0.0f);
	
	
	for (int i = 0; i < boidListLength; i++)
	{
	  if ((boidList[i]->location.getDistance(location)) <= range)
	  {
		numOfBoidsNearby+=1;
		sumOfVecs+=boidList[i]->velocity;
		
	  }
	   
	}
	
	//BoidVec3d avgVec = sumOfVecs/=(float)numOfBoidsNearby;

	/*if (numOfBoidsNearby != 1)
	{
		BoidVec3d avgVec = BoidVec3d(sumOfVecs.x/(float)numOfBoidsNearby, sumOfVecs.y/(float)numOfBoidsNearby, sumOfVecs.z/(float)numOfBoidsNearby);
		
		avgVec*=scalingFactor;
		
		return avgVec;	
	}
	else
	{
		return BoidVec3d(0.0f, 0.0f, 0.0f);
	}*/

	//BoidVec3d avgVec = BoidVec3d(sumOfVecs.x/(float)numOfBoidsNearby, sumOfVecs.y/(float)numOfBoidsNearby, sumOfVecs.z/(float)numOfBoidsNearby);
	
	
	BoidVec3d avgVec = sumOfVecs/(float)numOfBoidsNearby;
	avgVec*=scalingFactor;
	
	return avgVec;
	
}

BoidVec3d Boid::cohesion(std::vector<Boid*> boidList, float range, float scalingFactor, int boidListLength)
{
	float locAvgX = 0;
	float locAvgY = 0;
	float locAvgZ = 0;

	int numOfBoids = 0;
	
	for (int i = 0; i < boidListLength; i++)
	{
	  if (boidList[i]->location.getDistance(location) < range)
	  {
		locAvgX+= boidList[i]->location.x;
		locAvgY+= boidList[i]->location.y;
		locAvgZ+= boidList[i]->location.z;
		numOfBoids+=1;
		
	  }
	  
	}
	
	locAvgX /= (float)numOfBoids;
	locAvgY /= (float)numOfBoids;
	locAvgZ /= (float)numOfBoids;    
	
	BoidPosition3d avgLoc = BoidPosition3d(locAvgX, locAvgY, locAvgZ, false);
	
	BoidVec3d vecFromMeToAvgLoc = BoidVec3d(location, avgLoc);
	
	vecFromMeToAvgLoc*=scalingFactor;
	return vecFromMeToAvgLoc;
}